using System;
using System.Collections.Generic;
using System.Text;
using MyMath;

namespace Physics
{
    class ConstraintGroup:ConstrainingObject
    {
        List<ConstrainingObject> objects = new List<ConstrainingObject>();

        public List<ConstrainingObject> Objects
        {
            get { return objects; }
        }

        public void AddObject(ConstrainingObject obj)
        {
            objects.Add(obj);
        }
        public void RemoveObject(ConstrainingObject obj)
        {
            objects.Remove(obj);
        }

        public override bool IntersectSphere(MyMath.MyVector position, MyMath.MyVector velocity, CollisionSphere sphere,
            out MyMath.MyVector intersectionPoint, out MyMath.MyVector intersectionNormal,out MyVector translateBack)
        {
            MyVector collisionPoint;//, intersectionPoint;
            MyVector collisionNormal;//, intersectionNormal;
            MyVector translation = new MyVector(0, 0, 0);

            collisionPoint=new MyVector(0,0,0);
            int nIP = 0;
            collisionNormal=new MyVector(0,0,0);
            foreach(ConstrainingObject obj in objects)
            {
                bool status = obj.IntersectSphere(position, velocity, sphere, out intersectionPoint, out intersectionNormal,out translateBack);
                if (status)
                {
                    collisionPoint += intersectionPoint;
                    collisionNormal += intersectionNormal;
                    translation += translateBack;
                    nIP++;
                }
            } 
            if (nIP > 1)
            {
                collisionPoint.Divide(nIP);
                collisionNormal.Normalize();
            }
            intersectionPoint = collisionPoint;
            intersectionNormal = collisionNormal;
            translateBack = translation;
            return nIP > 0;
        }

        public override bool IntersectPoint(MyMath.MyVector position, MyMath.MyVector velocity, out MyMath.MyVector intersectionPoint,
            out MyMath.MyVector intersectionNormal, out MyVector translateBack)
        {
            MyVector collisionPoint;//, intersectionPoint;
            MyVector collisionNormal;//, intersectionNormal;

            MyVector translation = new MyVector(0, 0, 0);

            collisionPoint = new MyVector(0, 0, 0);
            int nIP = 0;
            collisionNormal = new MyVector(0, 0, 0);
            foreach (ConstrainingObject obj in objects)
            {
                bool status = obj.IntersectPoint(position, velocity, out intersectionPoint, out intersectionNormal,out translateBack);
                if (status)
                {
                    collisionPoint += intersectionPoint;
                    collisionNormal += intersectionNormal;
                    translation += translateBack;
                    nIP++;
                }
            }
            if (nIP > 1)
            {
                collisionPoint.Divide(nIP);
                collisionNormal.Normalize();
            }
            intersectionPoint = collisionPoint;
            intersectionNormal = collisionNormal;
            translateBack = translation;
            return nIP > 0;
        }


    }
}
